/*
*******************************************************************************
* ansi c source code
* file name:
*	sync2.c
* abstract:
*	synchronization
* reference:
* author:Fangming He
*******************************************************************************
*/

#include<stdlib.h>
#include<stdio.h>
#include<math.h>
#include<string.h>
#include "../common/globaldef.h"
#include "SRFFT.h"
//#define GRANGE  5
#include "../common/globalVai.h"


/* the first operand is conjugated */
struct Complex MulComplex(struct Complex m1, struct Complex m2)
{
	struct Complex product;
	product.real = (m1.real * m2.real + m1.imag * m2.imag);
	product.imag = (m1.real * m2.imag - m1.imag * m2.real);
	return product;
}

struct Complex MulComplex2(struct Complex m1, struct Complex m2)
{
	struct Complex product;
	product.real = (m1.real * m2.real - m1.imag * m2.imag);
	product.imag = (m1.real * m2.imag + m1.imag * m2.real);
	return product;
}
/* conjugate */
struct Complex ConComplex(struct Complex c1)
{
	struct Complex conjugate;
	conjugate.real = c1.real;
	conjugate.imag = -c1.imag;
	return conjugate;
}

struct Complex SumComplex(struct Complex a1, struct Complex a2)
{
	struct Complex sum;
	sum.real = a1.real + a2.real;
	sum.imag = a1.imag + a2.imag;
    return sum;
}

double Modulo(struct Complex sample)
{
	return ( sample.real * sample.real + sample.imag * sample.imag );
}

double ComputeMd(int d, double *angle, struct Complex *Symbol2)
{
	int m;
	int L = FFT_Size/2;
	double Md;
	double SumRd;
	struct Complex SumPd;
	struct Complex temp;

	SumPd.real = 0;
	SumPd.imag = 0;
	SumRd = 0;
	for( m = 0; m<L; m++)
	{ 
		temp = MulComplex(Symbol2[d+m],Symbol2[d+m+L]);
		SumPd = SumComplex(SumPd,temp);
        		 
		SumRd = SumRd + Modulo(Symbol2[d+m+L]);
	}

	// get the angle -PI~PI
	if ( SumPd.real == 0)
	{
		(*angle) = PI/2;
	}
	else
	{
		(*angle) = atan(SumPd.imag/SumPd.real);
	}
    if (SumPd.real < 0 && SumPd.imag < 0)
	{
        (*angle) -= PI;
	}

    if (SumPd.real < 0 && SumPd.imag > 0)
	{
        (*angle) += PI;
	}


    Md = Modulo(SumPd)/(SumRd * SumRd);
	return Md;
}

double ComputeBg(int g, struct Complex *X1, struct Complex *X2, struct Complex *V)
{
	int k;
	int index;
	double Bg;
	double SumDown;
    struct Complex SumUp;
	struct Complex temp;

	SumDown = 0;
	SumUp.real = 0;
	SumUp.imag = 0;
	for( k = 0; k < FFT_Size; k = k+2)
	{
		index = (k+2*g)%FFT_Size;// need to discuss
		temp = MulComplex2(X1[index], V[k]);
		temp = ConComplex(temp);
		temp = MulComplex2(temp, X2[index]);
		SumUp = SumComplex(SumUp, temp);

		SumDown = SumDown + Modulo(X2[k]);
	}
    Bg = Modulo(SumUp)/(2*SumDown*SumDown);
	return Bg;
}


int Sync(int *StartIndex, double *Offset_Esti ,int *flag ,struct Complex *Symbol)
{
	int i;
	int L = FFT_Size/2;
	static int d = 0;
	int g;
	double *Bg;// used to save the value of B(g)
	double *Md;// used to save the value of M(d)
	double angle;
	static double Offset_frac = 0;
	//static struct Complex Symbol2[2*(FFT_Size+CP_Length)];
	struct Complex *FirstSymbol;
	struct Complex *SecondSymbol;
	struct Complex *FreqComp;

	Md = (double *) malloc ( (CP_Length + 1) * sizeof (double));
	if( Md  == NULL)
	{
		printf("malloc Md  error\n");
		return 0;
	}
	memset(Md, 0, (CP_Length + 1) * sizeof (double));

	Bg = (double *) malloc ( FFT_Size * sizeof (double));
	if( Bg  == NULL)
	{
		printf("malloc Bg  error\n");
		return 0;
	}
	memset(Bg,0,FFT_Size * sizeof (double));


	FirstSymbol = (struct Complex *) malloc ( FFT_Size * sizeof (struct Complex));
	if( FirstSymbol  == NULL)
	{
		printf("malloc FirstSymbol  error\n");
		return 0;
	}
	memset(FirstSymbol,0,FFT_Size * sizeof (struct Complex));

	SecondSymbol = (struct Complex *) malloc ( FFT_Size * sizeof (struct Complex));
	if( SecondSymbol  == NULL)
	{
		printf("malloc SecondSymbol  error\n");
		return 0;
	}
	memset(SecondSymbol,0,FFT_Size * sizeof (struct Complex));

	FreqComp = (struct Complex *) malloc ( FFT_Size * sizeof (struct Complex));
	if( FreqComp  == NULL)
	{
		printf("malloc FreqComp  error\n");
		return 0;
	}
	memset(FreqComp,0,FFT_Size * sizeof (struct Complex));

	/* construct a symbol of a length of 2*(FFT_Size+CP_Length) */
	for( i = 0; i<(FFT_Size+CP_Length); i++ )
	{
		Symbol2[(FFT_Size+CP_Length) + i] = Symbol2[i];
	}
	for( i = 0; i<(FFT_Size+CP_Length); i++ )
	{
        Symbol2[i] = Symbol[i];
	}

    //SRFFT(Symbol+96,1024);
    if( ComputeMd(CP_Length, &angle, Symbol2) > Threshold )//there exist the first training symbol
	{
        (*flag) = 1;
		//search the best timing point
		for( d = 0; d <= CP_Length; d++ )
		{
			/*
			if( ComputeMd(d, &angle, Symbol2) > Threshold )
			{
				(*StartIndex) = d;
                (*flag) = 1;
	            //calculate the fragment part of the frequency offset 
                //Offset_frac = angle/(PI*FFT_Size);
                Offset_frac = angle/PI;
				break;
			}
			*/
			Md[d] = ComputeMd(d, &angle, Symbol2);
		}

		//find the index of the maximal value of Md
		d = 0;
		for ( i = 1; i <= CP_Length; i++)
		{
			if ( Md[i] > Md[d])
			{
				d = i;
			}
		}
		(*StartIndex) = d;
        ComputeMd(d, &angle, Symbol2);//get the value of angle
        Offset_frac = angle/PI;

	}
	else if( ComputeMd((FFT_Size+CP_Length)+CP_Length, &angle, Symbol2) > Threshold )//there exist two training symbols 
	{
		//use the estimated best timing point d caculated before
		//use the fragment part of frequency offset calculated before
		//partially correct the fragment part of frequency offset in two symbols
		//FFT
		//find the integer part

		//get the first symbol and the second symbol according to d
		for( i = 0; i < FFT_Size; i++)
		{
			FirstSymbol[i] = Symbol2[(FFT_Size+CP_Length) + d + i];//(FFT_Size+CP_Length)+d+i will not larger than 2239
			SecondSymbol[i] = Symbol2[d + i];
		}
		//construct the frequency compensate array
		for( i = 0; i < FFT_Size; i++)
		{
			FreqComp[i].real = cos(2*PI*Offset_frac*i/FFT_Size);
			FreqComp[i].imag = -sin(2*PI*Offset_frac*i/FFT_Size);
		}
        //frequency compensate
		for( i = 0; i < FFT_Size; i++)
		{
			FirstSymbol[i] = MulComplex2(FirstSymbol[i], FreqComp[i]);
			SecondSymbol[i] = MulComplex2(SecondSymbol[i], FreqComp[i]);
		}
		//FFT
        SRFFT(FirstSymbol, FFT_Size);
		SRFFT(SecondSymbol, FFT_Size);
		//calulate B(g) to find g
		for ( g = 0; g < FFT_Size; g++)
		{
			Bg[g] = ComputeBg(g,FirstSymbol,SecondSymbol,PN_Diff);
		}
		//find the index of the maximal value of Bg
		g = 0;
		for ( i = 1; i < FFT_Size; i++)
		{
			if ( Bg[i] > Bg[g])
			{
				g = i;
			}
		}
		// return the total frequency offset
        (*Offset_Esti) = 2*g + Offset_frac;
	}
	else
	{
		//do nothing
	}

	free(Bg);
	free(FirstSymbol);
	free(SecondSymbol);
	free(FreqComp);

    return 1;
}

